# 手腕控制单例
# 也行不用使用class增加复杂性，使用函数就可以啦？？？
# 增加串口断开自动重连的机制
# 使用Python官方推荐的singleton
import threading
import serial
import time
from PyQt5.QtCore import QThread, QObject, pyqtSignal, QTime
from loguru import logger
from serial.serialutil import SerialBase, SerialException, to_bytes, portNotOpenError, writeTimeoutError

import Lobot
from Config import Config
from singleton import singleton

import byte_util
from time import sleep

from utils import approximate

LOBOT_SERVO_FRAME_HEADER = 0x55


class WorkerSignals(QObject):
    '''
    Defines the signals available from a running worker thread.

    Supported signals are:

    finished
        No data

    error
        `tuple` (exctype, value, traceback.format_exc() )

    result
        `object` data returned from processing, anything

    progress
        `int` indicating % progress

    '''
    finished = pyqtSignal()
    error = pyqtSignal(str)
    result = pyqtSignal(object)
    progress = pyqtSignal(tuple)


@singleton
class Wrist(object):
    # lock
    _read_lock = threading.Lock()
    _write_lock = threading.Lock()

    # param
    _position = 0
    target_angle = 0

    # serial
    ser = serial.Serial()
    ser.baudrate = Config().inst().getint("lobot", "baudrate")
    # ser.port = "COM8"
    ser.port = Config().inst().get("lobot", "port")
    ser.timeout = Config().inst().getint("lobot", "timeout")

    dev_id = Config().inst().getint("lobot", "dev_id")
    interrupt_flag = False

    signals = WorkerSignals()

    def __init__(self):
        self.init()
        pass

    def init(self):
        try:
            self.ser.open()
        except SerialException as e:
            print(e)
            self.signals.error.emit("{}".format(e))
        else:
            self.signals.error.emit("")
        finally:
            pass

    def home(self):
        self.move_to({"angle": 615, "duration": 3000})

    def move_to(self, params):
        self.target_angle = params["angle"]
        with Wrist._write_lock:
            Lobot.LobotSerialServoMove(self.ser, self.dev_id, params["angle"], params["duration"])

        return True

    def home(self):
        pass

    def read_angle(self):
        with Wrist._write_lock:
            return Lobot.LobotSerialServoReadPosition(self.ser, self.dev_id)

    @property
    def position(self):
        return self._position

    @position.setter
    def position(self, position):
        if isinstance(position, int):
            self._position = position
        else:
            print("error not int")

    def interrupt(self, flag):
        self.interrupt_flag = flag

    def is_interrupted(self):
        return self.interrupt_flag

    def reach(self):
        angle = self.read_angle()
        if approximate(self.target_angle, angle, 5):
            return True
            # if (self.target_angle - 5) < angle < (self.target_angle + 5):

        return False

    def do_action(self, params, action):
        self.interrupt(False)
        self.move_to(params)
        return True

    def wait_action(self, params):
        # logger.info(params)
        wait_timeout = 10

        if "wait_timeout" in params:
            wait_timeout = params["wait_timeout"]

        timeout = QTime.currentTime().addSecs(wait_timeout)
        while not self.reach():
            if self.is_interrupted():
                return False
            sleep(0.5)
            if QTime.currentTime() > timeout:
                logger.error("Wrist Move Timeout!")
                return False

        return True
